#ifndef DYNAMIC_HIT_OUTPOST_H
#define DYNAMIC_HIT_OUTPOST_H

#define YAWLENGTH 100
#include <iostream>
#include <mutex>

#include "../queues/circlequeue.h"
#include "../../../tool/include/types.h"
// #include "global.h"
// #include "types.h"
// #include "util_func.h"
// #include "macros.h"
// #include "base.h"

// 对于小陀螺建立一个从自己观测的一个圆弧，运动模型
// 对于变化的量用一阶滞后滤波， 比如陀螺周期
// 对于基本不变的量用平均值

class DynamicHitOutpost {

 public:
    /*--------------------------*/ 
    int judgeNum=0;
    bool judgeFirst=true;

    //用来记录这个装甲板的yaw数据
    std::vector<cv::Point2d> yaw_recording;
    // 用来记录装甲板的转换矩阵
    std::vector<cv::Mat> mat_k;
    // 中心点的容器
    std::vector<cv::Point2d> centerPoints;
    // 预测时间
    double prTime[3]={0.0};
    // 间距
    double delTime=0;
    // 可以开火
    bool fireBegin=false;
    //拟合装甲板的阶数
    int jie=1;
    // 中心yaw数据
    double centerYaw;
    // 中心目标数值
    Target centerTarget;
 
   
    //对装甲板数据进行拟合
    cv::Mat polyfit(std::vector<cv::Point2d>& in_point, int n);
    double getResult(cv::Mat mat_k,int n,double inNum);
    double solveEquations(cv::Mat mat_k,int n,double targetNum,double solution,int minNum,int maxNum);
    void calculate_outpost_info_new();
    /*--------------------------*/ 




    const double outpost_period = 2.5;// 单位s
    const int armor_num = 3;

   /// 判断当前时间是否是正确的击打时间
    bool rightTime(int64_t now,double flyTime);
    
   //  返回正确的区间个数
    int getTimes(int64_t now,double flyTime,bool right_info);

   /// 最小二乘法使用函数
    double fun(int n, double array[YAWLENGTH][YAWLENGTH]);
    void  LeastSquareMethod(int n,int m, double* x, double* y,double &aYaw,double &bYaw);
    /// 陀螺数据存储类
    OutpostDescription outpost_description;

    /// 最近一次装甲板切换的时刻
    int64_t last_armor_switch_clock = 0.0;

    /// 记录车辆装甲板状态，在切换装甲板时不清空，只有在切换车辆的时候才会清空
    CircleQueue<InputOfDynamicOutpost, 100> car_armors_data;

    /// 记录车辆装甲板状态，在切换装甲板时清空，只有在切换车辆
    CircleQueue<InputOfDynamicOutpost, 100> same_armor_data;

    // 装甲板数量

    // 云台最大角速度设定值，单位度/s
    double gimbal_max_yaw_speed = 90;

    /// 记录装甲板切换的时间戳， 单位ms
    CircleQueue<int64_t, 50> armor_switch_clock_recorder;

    /// 记录左侧切换装甲板数据
    CircleQueue<Target, 5> left_switch_targets;

    /// 记录右侧切换装甲板数据
    CircleQueue<Target, 5> right_switch_targets;

    bool flag_init_yaw = false;

    bool flag_init_average_yaw = false;

 public:
    void update(const InputOfDynamicOutpost &data);


    //数据更新函数
    void update_new(InputOfDynamicOutpost &data);


   //动态旋转前哨站数据更新函数
    void update_new_dynatic(InputOfDynamicOutpost &data);
   // 计算中心center对应时间
   //  void calculateYawSave();

    void judge_outpost_status_when_armor_switched(double distance);

    /**
    * @brief 判断陀螺描述器是否在时间上已经过期
    * @details 用于在长时间检测不到装甲板的时候及时退出陀螺状态
    */
    bool if_outpost_description_updated(int64_t now);

    /// 切换车辆时清空
    void clear_if_car_switched();

    /// 切换装甲板
    void clear_if_armor_switched();

    /**
    * @brief 计算小陀螺状态
    * @details 包括陀螺是否有效/陀螺状态/陀螺周期/左右两侧装甲板对应的yaw宽度
    * @details 敌方机器人平面平移运动带来的yaw观测角速度
    * @details 平均z高度/左右两侧装甲板对应的z高度
    */
    void calculate_outpost_info();

    /// 跟随击打前哨战
    void follow_hit_outpost(double v, int64_t now, SendPack &send_pack);

   /// 跟随击打前哨战
    void follow_hit_outpost_new(double v, int64_t now, SendPack &send_pack);
    /// 瞄准中心击打前哨战
    void aim_center_hit_outpost(double v, int64_t now, SendPack &send_pack);

    void aim_center_hit_outpost_new(double v, int64_t now, SendPack &send_pack);


    /**
    * @brief 图形化展示当前陀螺计算的一些信息
    * @details 包括陀螺的中心/转速/旋转方向/预测击打点/过去对现在的预测等信息
    */
    void show_result(cv::Mat &canvas, double v, int64_t now);

    // 计算一个区间对应的时间跨度
    void get_time_span(double &time_span);

    double get_average_xy_distancce();

    double get_average_z_distancce();

    double get_average_same_armor_z_distancce();

    void judge_rotate_dir(double yaw_speed, ArmorDir &dir);
};

#endif// DYNAMIC_HIT_OUTPOST_H